{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "4278f0a7-30cf-422f-b4aa-2bdd90db7299",
   "metadata": {},
   "source": [
    "# 单电机实验 demo"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "72147c7a-2d49-49fb-87b7-f40b02bf1b74",
   "metadata": {},
   "source": [
    "## 初始化电机"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "ae0d945a-b02e-4062-a10d-f6655834d8b2",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "from robodyno.components import Motor\n",
    "from robodyno.interfaces import CanBus\n",
    "can = CanBus()\n",
    "\n",
    "motor = Motor(can, 0x10)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "abaf865c-2c3a-4c5a-ba4c-08de67f14ba3",
   "metadata": {},
   "source": [
    "## 使能"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "600e1068-b22a-4d4f-ac7e-1893d5ff46c0",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "motor.enable()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "82f0c162-db8a-4264-838d-d2c8c4db0fb3",
   "metadata": {},
   "source": [
    "## 控制"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "e84872a4-60b2-446e-a7a6-97c250fcd1de",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "3739a6779efc45e0a30249718a7487b7",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "ToggleButtons(description='控制对象:', options=(('位置', 3), ('速度', 2), ('力矩', 1)), value=3)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "d1c2c43b1248439d88bcb9f3e45dd282",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "ToggleButtons(description='控制模式:', index=2, options=(('直接', 31), ('滤波', 33), ('匀速', 35)), value=35)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "e73c0b9fc1ad457e8bc73ffbff3f4097",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "HBox(children=(Label(value='位置(rad)'), FloatSlider(value=0.0, max=3.14, min=-3.14, step=0.01)), layout=Layout(…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "90b32e4d55cc438a9289474f612ee859",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "VBox(children=(HBox(children=(Label(value='电压: 0.00 V'), Label(value='温度: 0.00 °C'))), HBox(children=(Label(va…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "325a8a45674b4aca91985c1acd28d819",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "ToggleButtons(description='实时曲线:', options=(('位置', 3), ('速度', 2), ('力矩', 1)), value=3)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "8ed59463daa2409f89a4fd2cdf9ec1dd",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Figure(axes=[Axis(label='时间(s)', scale=LinearScale()), Axis(label='位置(rad)', orientation='vertical', scale=Lin…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import ipywidgets as widgets\n",
    "from IPython.display import display\n",
    "import bqplot.pyplot as plt\n",
    "import time \n",
    "import threading\n",
    "import numpy as np\n",
    "\n",
    "init_time = time.time()\n",
    "\n",
    "thread_running = True\n",
    "\n",
    "cmode, imode = motor.get_mode()[0].value\n",
    "origin_pos = motor.get_pos()\n",
    "\n",
    "target_selector = widgets.ToggleButtons(\n",
    "    options=[('位置', 3), ('速度', 2), ('力矩', 1)],\n",
    "    description='控制对象:',\n",
    "    value = cmode\n",
    ")\n",
    "mode_options = [\n",
    "    [], [('直接', 11)],\n",
    "    [('直接', 21), ('匀加减速', 22)],\n",
    "    [('直接', 31), ('滤波', 33), ('匀速', 35)],\n",
    "]\n",
    "mode_selector = widgets.ToggleButtons(\n",
    "    options=mode_options[target_selector.value],\n",
    "    description='控制模式:',\n",
    "    value = cmode*10+imode\n",
    ")\n",
    "\n",
    "labels = ['', '力矩(Nm)', '速度(rad/s)', '位置(rad)']\n",
    "slider_label = widgets.Label(value=labels[target_selector.value])\n",
    "pos_slider = widgets.FloatSlider(\n",
    "    min=-3.14,\n",
    "    max=3.14,\n",
    "    step=0.01,\n",
    "    readout_format='.2f',\n",
    ")\n",
    "vel_slider = widgets.FloatSlider(\n",
    "    min=-3.14,\n",
    "    max=3.14,\n",
    "    step=0.01,\n",
    "    readout_format='.2f',\n",
    ")\n",
    "torque_slider = widgets.FloatSlider(\n",
    "    min=-0.3,\n",
    "    max=0.3,\n",
    "    step=0.001,\n",
    "    readout_format='.3f',\n",
    ")\n",
    "sliders = [None, torque_slider, vel_slider, pos_slider]\n",
    "slider_box = widgets.HBox(\n",
    "    [slider_label, sliders[target_selector.value]],\n",
    "    layout={'margin': '24px 0 12px 0'}\n",
    ")\n",
    "\n",
    "vbus_label = widgets.Label(value='电压: {:.2f} V'.format(0))\n",
    "temp_label = widgets.Label(value='温度: {:.2f} °C'.format(0))\n",
    "pos_label = widgets.Label(value='位置: {:.4f} rad'.format(0))\n",
    "vel_label = widgets.Label(value='速度: {:.4f} rad/s'.format(0))\n",
    "torque_label = widgets.Label(value='力矩: {:.4f} Nm'.format(0))\n",
    "info_label_box = widgets.VBox([\n",
    "    widgets.HBox([vbus_label, temp_label]), \n",
    "    widgets.HBox([pos_label, vel_label, torque_label])\n",
    "])\n",
    "\n",
    "plot_selector = widgets.ToggleButtons(\n",
    "    options=[('位置', 3), ('速度', 2), ('力矩', 1)],\n",
    "    description='实时曲线:',\n",
    ")\n",
    "\n",
    "plot_x_vals = []\n",
    "plot_y_vals = [[], [], [], []]\n",
    "\n",
    "fig = plt.figure(animation_duration=0)\n",
    "axes_options = {\"x\": {\"label\": \"时间(s)\"}, \"y\": {\"label\": \"位置(rad)\"}}\n",
    "lines = plt.plot(x=plot_x_vals, y=plot_y_vals[plot_selector.value], axes_options=axes_options)\n",
    "\n",
    "def on_target_change(change):\n",
    "    global origin_pos\n",
    "    if change.new == 3:\n",
    "        origin_pos = motor.get_pos()\n",
    "        motor.set_pos(origin_pos)\n",
    "    elif change.new == 2:\n",
    "        motor.set_vel(0)\n",
    "    elif change.new == 1:\n",
    "        motor.set_torque(0)\n",
    "    mode_selector.options = mode_options[change.new]\n",
    "    slider_label.value = labels[change.new]\n",
    "    sliders[change.new].value = 0\n",
    "    slider_box.children = [slider_label, sliders[change.new]]\n",
    "target_selector.observe(on_target_change, names='value')\n",
    "\n",
    "def on_mode_change(change):\n",
    "    if change.new == 31:\n",
    "        pos_slider.continuous_update = False\n",
    "        motor.position_mode()\n",
    "    elif change.new == 33:\n",
    "        pos_slider.continuous_update = True\n",
    "        motor.position_filter_mode(3)\n",
    "    elif change.new == 35:\n",
    "        pos_slider.continuous_update = False\n",
    "        motor.position_track_mode(2,0.5,0.5)\n",
    "    elif change.new == 21:\n",
    "        motor.velocity_mode()\n",
    "    elif change.new == 22:\n",
    "        motor.velocity_ramp_mode(0.5)\n",
    "    elif change.new == 11:\n",
    "        motor.torque_mode()\n",
    "mode_selector.observe(on_mode_change, names='value')\n",
    "\n",
    "def on_pos_slider_change(change):\n",
    "    motor.set_pos(change.new + origin_pos)\n",
    "pos_slider.observe(on_pos_slider_change, names='value')\n",
    "def on_vel_slider_change(change):\n",
    "    motor.set_vel(change.new)\n",
    "vel_slider.observe(on_vel_slider_change, names='value')\n",
    "def on_torque_slider_change(change):\n",
    "    motor.set_torque(change.new)\n",
    "torque_slider.observe(on_torque_slider_change, names='value')\n",
    "\n",
    "display(target_selector, mode_selector, slider_box, info_label_box, plot_selector, fig)\n",
    "\n",
    "def read_pos_thread():\n",
    "    while thread_running:\n",
    "        vbus = motor.get_voltage()\n",
    "        temp = motor.get_temperature()\n",
    "        pos, vel, torque = motor.get_feedback()\n",
    "        vbus_label.value='电压: {:.2f} V'.format(vbus)\n",
    "        temp_label.value='温度: {:.2f} °C'.format(temp)\n",
    "        pos_label.value='位置: {:.4f} rad'.format(pos)\n",
    "        vel_label.value='速度: {:.4f} rad/s'.format(vel)\n",
    "        torque_label.value='力矩: {:.4f} Nm'.format(torque)\n",
    "        \n",
    "        plot_y_vals[1].append(torque)\n",
    "        plot_y_vals[2].append(vel)\n",
    "        plot_y_vals[3].append(pos)\n",
    "        plot_x_vals.append((time.time() - init_time))\n",
    "        \n",
    "        lines.x = plot_x_vals\n",
    "        lines.y = plot_y_vals[plot_selector.value]\n",
    "        plt.ylabel(labels[plot_selector.value])\n",
    "\n",
    "        time.sleep(0.05)\n",
    "\n",
    "fig_thread = threading.Thread(target=read_pos_thread)\n",
    "\n",
    "fig_thread.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0e801b52-ba80-46d4-b095-51f11c573ef6",
   "metadata": {},
   "source": [
    "## 停止"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "5e73ab89-3e0f-4820-8efe-e5730601fe4d",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "thread_running = False\n",
    "fig_thread.join()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "60b9dc14-ae16-441c-add3-71efb49be2fd",
   "metadata": {},
   "source": [
    "## 电机失能"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "2eee1033-526b-4541-bf2f-472329ffe242",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "motor.disable()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c7d4172a-e37f-4283-8f21-852fb063db86",
   "metadata": {},
   "source": [
    "## 断开CAN总线"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "c3ea31d2-16cd-4a99-8c85-44d311d462cc",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "can.disconnect()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
